package init_tests;

import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;

public class NxtPreStartTests {

	// vars

	// constructors
	public NxtPreStartTests() {
		super();
	}

	// methods
	public void testAll() throws IllegalStateException {
		System.out.println("tests starting");
		System.out.println("");

		testBattery();
		testSensors();
	}

	public void testBattery() throws IllegalStateException {
		// try to figure out if there is a battery connected
		System.out.println("testing battery");
		// TODO: Insert battery test here!
		System.out.println("battery ok");
	}

	public void testSensors() throws IllegalStateException {
		System.out.println("testing sensors");

		// testing ultrasonic on port1
		System.out.print("us/p1... ");
		UltrasonicSensor ultraSonicSensor1 = new UltrasonicSensor(SensorPort.S1);
		boolean isUltraSonic1 = ultraSonicSensor1.getProductID()
				.equals("Sonar");
		if (isUltraSonic1) {
			System.out.println("OK");
		} else {
			System.out.println("ERROR");
			throw new IllegalStateException("us/p1 missing!");
		}

		// testing ultrasonic on port2
		System.out.print("us/p2... ");
		UltrasonicSensor ultraSonicSensor2 = new UltrasonicSensor(SensorPort.S2);
		boolean isUltraSonic2 = ultraSonicSensor2.getProductID()
				.equals("Sonar");
		if (isUltraSonic2) {
			System.out.println("OK");
		} else {
			System.out.println("ERROR");
			throw new IllegalStateException("us/p2 missing!");
		}

		// testing start/stop button on port3
		System.out.print("eb/p3... ");
		TouchSensor touchSensor3 = new TouchSensor(SensorPort.S3);

		outer: while (true) {
			for (int i = 0; i < 1000; i++) {
				if (!touchSensor3.isPressed()) {
					try {
						Thread.sleep(10);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
				} else {
					System.out.println("OK");
					break outer;
				}
			}
			System.out.println("ERROR");
			throw new IllegalStateException("eb/p3 missing!");
		}

		// testing frontbumper button on port4
		System.out.print("fb/p4... ");
		TouchSensor touchSensor4 = new TouchSensor(SensorPort.S4);

		outer: while (true) {
			for (int i = 0; i < 1000; i++) {
				if (!touchSensor4.isPressed()) {
					try {
						Thread.sleep(10);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
				} else {
					System.out.println("OK");
					break outer;
				}
			}
			System.out.println("ERROR");
			throw new IllegalStateException("fb/p4 missing!");
		}

	}

}
